#!/usr/bin/python3
# -*- coding: utf-8 -*-
import os
import sys
import time
from multiprocessing import Array, Process, Queue
from multiprocessing.managers import BaseManager

import numpy as np
from IPython import embed

from Devel.ZenWorld import StartRobotWorld, Robot
from Devel.Trajectory import ComputeTrajectory, PlotTraj

if __name__ == "__main__":
    CWD = os.path.dirname(__file__)
    CWD = os.getcwd()
    model_path1 = os.path.join(CWD, "Models/GP12/GP12.urdf")
    model_path2 = os.path.join(CWD, "Models/Motomini/Motomini.urdf")
    r1 = Robot(model_path1)
    world = StartRobotWorld()
    world.LoadRobot(r1)

    pose = [0.5, 0, -0.5, 0, 0, 0, 1]  # xyz,quat
    r1.SetBaseTransformation(pose)
    home_joints = np.array([0, 0, 0, 0, -np.pi/2, 0])

    # move joints
    r1.MoveJ(home_joints)

    # move to a pose
    pose = r1.GetRobotPose()  # 4x4 transformation
    pose[2, 3] -= 0.5
    r1.MoveL(pose)

    # follow trajectory
    waypoints = np.array([
        r1.GetRobotJointPosition(),
        [-np.pi/6, -np.pi/4, -np.pi/5, np.pi/6, -np.pi/2, 0],
        np.random.uniform(-np.pi/2, np.pi/2, (6,))])
    traj = ComputeTrajectory(waypoints)
    # PlotTraj(traj)
    r1.FollowTraj(traj)

    # load another robot
    r2 = Robot(model_path2)
    world.LoadRobot(r2)
    r2.MoveJ([1, 0, 0, 0, 0, 0])

    # copy a robot and load it
    r3 = r1.Copy(r1.GetName() + "copy")
    world.LoadRobot(r3)
    r3.SetBaseTransformation([-1, 0, 0, 0, 0, 0, 1])
    print("world robots: ", world.GetRobots())

    embed()
